/* 
 * File:   Calculate.cpp
 * Author: song
 * 
 * Created on 2010年11月21日, 上午9:40
 */

#include "Calculate.h"

Calculate::Calculate(Camera *cam, trackObject *objsL, int size = 10) {
    this->otherCam = cam;
    this->objs.objsL = objsL;
    this->objSize = size;
    this->f = 825;

    float hranges_arr[]={0,180};
    float *hranges = hranges_arr;
    int hdims = 256;
    tmpobj.hist = cvCreateHist( 1, &hdims, CV_HIST_ARRAY, &hranges, 1 );
    tmpobj.image = cvCreateImage(cvGetSize(cam->pFrame), 8, 3);
    tmpobj.mask = cvCreateImage(cvGetSize(cam->pFrame), 8, 1);
    tmpobj.hsv = cvCreateImage( cvGetSize(cam->pFrame), 8, 3 );
    tmpobj.hue = cvCreateImage( cvGetSize(cam->pFrame), 8, 1 );
    tmpobj.backProject = cvCreateImage( cvGetSize(cam->pFrame), 8, 1 );
}

Calculate::Calculate(const Calculate& orig) {
}

Calculate::~Calculate() {
}

bool cmp(const trackObject &a, const trackObject &b)
{
    int ya = a.rect.y + a.rect.height/2;
    int yb = b.rect.y + b.rect.height/2;
    int t = ya - yb;
    if(t < 0) t = -t;
    if(t < 10) {
        ya = a.rect.x + a.rect.height/2;
        yb = b.rect.x + b.rect.height/2;
    }
    return ya < yb;
}

//参数计算模块的预处理部分，进行两幅画面中物体匹配和适别
//计算结果在index数组内，-1表示无匹配，否则为对应的bojsR中的下标
void Calculate::Match(trackObject objsL[], trackObject objsR[])
{
    //先按照y轴坐标差排序，y相差在10以内则进行x排序
    //待修改，数据结构不对
    sort(objsL, objsL, cmp);
    sort(objsR, objsR, cmp);

    int i, j, k, ln, rn;
    int ya, yb, t;
    int dis;
    //只扫描左边图像中的物体，进行匹配
    for(i = 0; i < ln; i++) {
        this->index[i] = -1;
        for(j = 0; j < rn; j++) {
            ya = objsL[i].rect.y + objsL[i].rect.height/2;
            yb = objsL[j].rect.y + objsL[j].rect.height/2;

            //y轴差当前扫描物体100%认为不可接受
            if(ya - yb > objsL[i].rect.height) continue;
            if(yb - ya > objsL[i].rect.height) break;

            //面积计算，如果变化率大于20%认为不是同一物体
            dis = objsL[i].rect.height * objsL[i].rect.width
                    - objsR[j].rect.height * objsL[j].rect.width;
            if(dis < 0) dis = -dis;
            if(dis / (objsL[i].rect.height * objsL[i].rect.width)
                    > 0.2) continue;
            
            this->index[i] = j;
        }
    }
}

//通过camshift进行两幅图像的匹配
void Calculate::Match()
{
    int i;
    trackObject *obj = &tmpobj;
    
    for(i = 0; i < this->objSize; i++) {
        if(objs.objsL[i].deleted == false) {
            *obj = objs.objsL[i];
            //在另外一个帧中计算camshift

            cvCopy(otherCam->pFrame, obj->image, 0);
            cvCvtColor(obj->image, obj->hsv, CV_BGR2HSV);
            cvInRangeS(obj->hsv, cvScalar(0, 30, 10, 0),
                    cvScalar(180, 256, 256,0), obj->mask );
            cvSplit(obj->hsv, obj->hue, 0, 0, 0);

            //设置图像的感兴趣区域
            cvSetImageROI(obj->hue, obj->rect);
            cvSetImageROI(obj->mask, obj->rect);

            cvCalcHist(&obj->hue, obj->hist, 0, obj->mask);
            float max_val = 0.0;
            cvGetMinMaxHistValue(obj->hist, 0, &max_val, 0, 0 );
            cvConvertScale(obj->hist->bins, obj->hist->bins,
                    max_val ? 255. / max_val : 0, 0 );
            cvResetImageROI(obj->hue);
            cvResetImageROI(obj->mask);
            //计算目标直方图的反向投影
            cvCalcBackProject(&(obj->hue), obj->backProject, obj->hist);

            //蒙板
            cvAnd(obj->backProject, obj->mask, obj->backProject, 0);
            //执行CamShift算法跟踪目标
            cvCamShift(obj->backProject, obj->rect,
                    cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ),
                    &obj->comp, &obj->box);
            
            //判断是否丢失
                if(obj->comp.rect.width <= 0 || obj->comp.rect.height <= 0
            || obj->box.size.width <= 0 || obj->box.size.height <= 0
            || obj->box.center.x < 0 || obj->box.center.x >= otherCam->pFrame->width
            || obj->box.center.y < 0 || obj->box.center.y >= otherCam->pFrame->height)
                {
                    objs.match[i] = false;
                    continue;
                }
            
            //判断是否匹配错误，通过简单面积的变化率
            int s1 = objs.objsL[i].comp.rect.height * objs.objsL[i].comp.rect.width;
            int s2 = obj->comp.rect.height * obj->comp.rect.width;
            int dis = s1-s2;
            if(dis < 0) dis = -dis;
            float var = (1.0*dis) / s1;
            
            if(var > 1.5 || var < 0.5 ) objs.match[i] = false;
            else {
                objs.match[i] = true;
                objs.dx[i] = obj->comp.rect.x - objs.objsL[i].comp.rect.x;
                if(objs.dx[i] < 0) objs.dx[i] = -objs.dx[i];
            }

        }
    }
}

//通过计算出的像素差计算视深
void Calculate::CalZDis()
{
    int i;
    for(i = 0 ; i < this->objSize; i++) {
        if(objs.match[i] == true) {
            objs.zDis[i] = (this->f*this->tx)/objs.dx[i];
            cout<<"objct:" + i<<"   ZDis "<<objs.zDis[i]<<endl;
        }
    }
}